#include <iostream>
#include <vector>
#include <numeric>
#include <opencv2/opencv.hpp>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"

/**
 * @brief 获取点云地图到云台手地图的平移和旋转矩阵
 * 
 * @param dst_point_set 目标点集
 * @param src_point_set 源点集
 * @param R 旋转矩阵
 * @param T 平移矩阵
 */
void getCoordinateTransformationMatrix(std::vector<cv::Point3d> dst_point_set, std::vector<cv::Point3d> src_point_set, cv::Mat &R, cv::Mat &T)
{
    // 检查输入
    if(dst_point_set.size() < 3 || src_point_set.size() < 3)
    {
        std::cout << "getCoordinateTransformationMatrix输入不满足条件" << 
            dst_point_set.size() << " " <<  src_point_set.size() << std::endl;
        return;
    }
    if(dst_point_set.size() != src_point_set.size())
    {
        auto size = std::min(src_point_set.size(), dst_point_set.size());
        dst_point_set.resize(size);
        src_point_set.resize(size);
    }

    // 开始计算
    cv::Mat src(src_point_set);  // 云台手点
    cv::Mat dst(dst_point_set);    // 点云点
    src = src.reshape(1, 0);
    dst = dst.reshape(1, 0);
    std::cout << "size: " << src.size() << "  " << dst.size() << std::endl;
    std::cout << src << std::endl << dst << std::endl;
    // 计算重心点
    cv::Point3d src_cm(0., 0., 0.);
    cv::Point3d dst_cm(0., 0., 0.);
    for (int i = 0; i < src.rows; i++)
    {
        cv::Point3d p(src.at<double>(i, 0), src.at<double>(i, 1), src.at<double>(i, 2));
        src_cm += p;
    }
    for (int i = 0; i < dst.rows; i++)
    {
        cv::Point3d q(dst.at<double>(i, 0), dst.at<double>(i, 1), dst.at<double>(i, 2));
        dst_cm += q;
    }
    src_cm /= src.rows;
    dst_cm /= dst.rows;

    // 将源点集和目标点集平移到重心处
    cv::Mat src_centered = src - cv::Mat::ones(src.rows, 1, CV_64F) * cv::Mat(src_cm).t();
    cv::Mat dst_centered = dst - cv::Mat::ones(dst.rows, 1, CV_64F) * cv::Mat(dst_cm).t();

    // 计算协方差矩阵
    cv::Mat W = cv::Mat::zeros(3, 3, CV_64F);
    for (int i = 0; i < src.rows; i++)
    {
        cv::Mat pi = src_centered.row(i).t();
        cv::Mat qi = dst_centered.row(i).t();
        W += qi * pi.t();
    }

    // 进行奇异值分解
    cv::Mat U, S, Vt;
    cv::SVD::compute(W, S, U, Vt, cv::SVD::MODIFY_A);

    // 计算旋转矩阵和平移矩阵
    R = Vt.t() * U.t();
    T = cv::Mat(dst_cm) - R * cv::Mat(src_cm);

    return;
}

/**
 * @brief 结果验证
 * 
 * @param x x坐标
 * @param y y坐标
 * @param z z坐标
 * @param R 旋转矩阵
 * @param T 平移矩阵
 * @return std::vector<double> 
 */
std::vector<double> coordinateTransform(float x, float y, float z, cv::Mat R, cv::Mat T)
{
    cv::Mat src = (cv::Mat_<double>(3, 1) << x, y, z);
    cv::Mat dst;
    dst = R * src + T;
    return std::vector<double>(dst.reshape(1, 1));
}


auto loadPointCloudMap(std::string file_path)
{
    cv::Mat pgm_img = cv::imread(file_path, cv::IMREAD_UNCHANGED);
    cv::Mat normalized_img;
    
    normalize(pgm_img, normalized_img, 0, 255, cv::NORM_MINMAX);

    normalized_img.convertTo(normalized_img, CV_8U);
    return normalized_img;
}

auto loadClientMap(std::string file_path)
{
    return cv::imread(file_path);
}

bool mouse_click_flag = false;
int mouse_x = 0;
int mouse_y = 0;
void mouse_callback(int event, int x, int y, int flag, void* params)
{
    if(event == cv::EVENT_LBUTTONDOWN)  //左键按下
    {
        mouse_click_flag = true;
        mouse_x = x;
        mouse_y = y;
    }
}

auto getMouseClicks()
{
    return cv::Point2i(mouse_x, mouse_y);
}

auto mouseClicked()
{
    if(mouse_click_flag)
    {
        mouse_click_flag = false;
        return true;
    }
    return false;
}

void operator<<(std::vector<cv::Point3d> &dst, std::vector<cv::Point2d> src)
{
    for(auto point : src)
    {
        cv::Point3d p(point.x, point.y, 0);
        dst.push_back(p);
    }
    return;
}

void operator<<(std::vector<cv::Point2d> &dst, std::vector<cv::Point2i> src)
{
    for(auto point : src)
    {
        cv::Point2d p(point.x, point.y);
        dst.push_back(p);
    }
    return;
}

std::vector<cv::Point3d> PerspectiveTransform(cv::Size client_map_size, std::vector<cv::Point2i> client_point_set)
{
    // 1.根据云台手地图和实际地图的比例关系，计算出图片上的点到实际点的坐标变换关系
    // 点顺序：从左下角开始，顺时针4个顶点
    std::vector<cv::Point2f> default_client_map_points = {cv::Point2d(0, client_map_size.height), cv::Point2d(0, 0), 
                    cv::Point2d(client_map_size.width, 0), cv::Point2d(client_map_size.width, client_map_size.height)};
    std::vector<cv::Point2f> default_real_map_points = {cv::Point2d(0, 0), cv::Point2d(0, 15), cv::Point2d(15, 28), cv::Point2d(0, 28)};
    auto perspectiveTransformMatrix_client2realMap = getPerspectiveTransform(default_client_map_points, default_real_map_points);
    // 2.将图片上的点转换成实际点
    std::vector<cv::Point2d> real_map_points;
    std::vector<cv::Point2d> client_map_points;
    client_map_points << client_point_set;
    cv::perspectiveTransform(client_map_points, real_map_points, perspectiveTransformMatrix_client2realMap);
    std::vector<cv::Point3d> real_map_points_3D;
    real_map_points_3D << real_map_points;
    return real_map_points_3D;
}

std::vector<cv::Point3d> goal_pose_points;
void getGoalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
    double x, y, z;
    x = msg->pose.position.x;
    y = msg->pose.position.y;
    z = msg->pose.position.z;

    cv::Point3d p(z, y, z);
    goal_pose_points.push_back(p);
}

rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr pub;

void main_thread()
{
    // auto point_cloud_map = loadPointCloudMap("/path/to/PointClouldMap");
    auto client_map = loadClientMap(MAP_STD_SRC_PATH);
    if(client_map.empty())
    {
        std::cout << "云台手小地图图片为空" << std::endl;
        return;
    }
    std::vector<cv::Point2i> client_point_set;

    //! point_count : 要点的点个数
    int point_count = 4;    
    int i = 0;
    
    cv::namedWindow("point cloud map");
    cv::setMouseCallback("point cloud map", mouse_callback, NULL);
    // 可视化标点 
    while (i < point_count)
    {
        cv::imshow("point cloud map", client_map);
        if(mouseClicked())
        {
            auto point = getMouseClicks();
            client_point_set.push_back(point);
            ++i;
            std::cout << "Point " << point << std::endl;
            cv::circle(client_map, point, 3, cv::Scalar(0, 255, 255));
        }
        if(cv::waitKey(10) == 'q') exit(0);
    }
    cv::imshow("point cloud map", client_map);
    std::cout << "图片采点结束" << std::endl;
    // 等rviz标出足够多的点
    while(goal_pose_points.size() < point_count) 
    {
        if(cv::waitKey(10) =='q') exit(0);
        if(!rclcpp::ok()) exit(0);
    };
    std::cout << "rviz采点结束" << std::endl;

    // 从像素坐标变换到云台手坐标
    auto real_points_set = PerspectiveTransform(client_map.size(), client_point_set);
    // 获得转换矩阵
    // 1.从云台手小地图坐标点变到导航地图坐标点
    cv::Mat R_client2cloud, T_client2cloud;
    getCoordinateTransformationMatrix(goal_pose_points, real_points_set, R_client2cloud, T_client2cloud);
    std::cout << "R_client2cloud\n" << R_client2cloud << "\nT_client2cloud\n" << T_client2cloud << std::endl;
    // 2.从导航地图坐标点变到云台手小地图坐标点
    cv::Mat R_cloud2client, T_cloud2client;
    getCoordinateTransformationMatrix(real_points_set, goal_pose_points, R_cloud2client, T_cloud2client);
    std::cout << "R_cloud2client\n" << R_cloud2client << "\nT_cloud2client\n" << T_cloud2client << std::endl;
    //保存
    cv::FileStorage map_config(MAP_TRANSFORMATION_PATH, cv::FileStorage::WRITE);
    map_config << "client2cloud" << "{";
    map_config << "R" << R_client2cloud;
    map_config << "T" << T_client2cloud;
    map_config << "}";
    map_config << "cloud2client" << "{";
    map_config << "R" << R_cloud2client;
    map_config << "T" << T_cloud2client;
    map_config << "}";
    map_config.release();
    // 验证
    auto ret = coordinateTransform(real_points_set[0].x, real_points_set[0].y, real_points_set[0].z, R_client2cloud, T_client2cloud);
    std::cout << "result : [" << ret[0] << ", " << ret[1] << ", " << ret[2] << "]" << std::endl;

    geometry_msgs::msg::PointStamped point;
    point.header.frame_id = "map";
    point.header.stamp = rclcpp::Clock().now();
    point.point.x = ret[0];
    point.point.y = ret[1];
    point.point.z = ret[2];
    pub->publish(point);
}

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("goal_pose_client");
    auto sub = node->create_subscription<geometry_msgs::msg::PoseStamped>("/goal_pose", 10, std::bind(getGoalPoseCallback, std::placeholders::_1));
    pub = node->create_publisher<geometry_msgs::msg::PointStamped>("/point", 10);
    std::thread t(main_thread);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}